#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/clocks.h"
#include "cmsis_os.h"
#define DEMO1 0
#define DEMOTEST 1

#if (USE_SMP==1u)
// task1
#define TASK1_STK_SIZE 4096
void task1(void* pdata);
osThreadDef(task1, osPriorityNormal+1, 1, TASK1_STK_SIZE);

// task2
#define TASK2_STK_SIZE 4096
void task2(void* pdata);
osThreadDef(task2, osPriorityNormal, 1, TASK2_STK_SIZE);

// task3
#define TASK1_STK_SIZE 4096
void task3(void* pdata);
osThreadDef(task3, osPriorityNormal, 1, TASK1_STK_SIZE);

// task4
#define TASK2_STK_SIZE 4096
void task4(void* pdata);
osThreadDef(task4, osPriorityNormal, 1, TASK2_STK_SIZE);

void task1(void* pdata)
{
    int count = 1;
     
    while (1) {
       printf("\r\nHello world!\r\n###This is task1 ,count is %d \r\n", count++);
        uint32_t id= port_GET_CORE_ID();
       printf("\r\nCoreNum is %d \r\n", id);
        //osDelay(1000);
    }
}
void task2(void* pdata)
{
    int count = 1;
    while (1) {
        printf("\r\nHello TobudOS !\r\n***This is task2 ,count is %d \r\n", count++);
        uint32_t id= port_GET_CORE_ID();
        printf("\r\nCoreNum is %d \r\n", id);
        //osDelay(1000);
    }
}
void task3(void* pdata)
{
    int count = 1;
    while (1) {
        printf("\r\nHello TobudOS !\r\n***This is task3 ,count is %d \r\n", count++);
        uint32_t id= port_GET_CORE_ID();
        printf("\r\nCoreNum is %d \r\n", id);
        //osDelay(1000);
    }
}
void task4(void* pdata)
{
    int count = 1;
    while (1) {
        printf("\r\nHello TobudOS !\r\n***This is task4 ,count is %d \r\n", count++);
        uint32_t id= port_GET_CORE_ID();
        printf("\r\nCoreNum is %d \r\n", id);
        //osDelay(1000);
    }
}
#endif

extern "C" void SysTick_Handler(void)
{
    if (tos_knl_is_running()) {
        //tos_knl_irq_enter();
        tos_tick_handler();
       // tos_knl_irq_leave();
    }
}
k_task_t *tasktest;
extern "C" int application_entry(void *arg);
osThreadDef(application_entry, osPriorityNormal, 1, 8192);
int main()
{
    stdio_init_all();
    while (!stdio_usb_connected())
     ; // Wait for USB to be connected
    printf("System Clock Frequency is %d Hz\n", clock_get_hz(clk_sys));
    printf("USB Clock Frequency is %d Hz\n", clock_get_hz(clk_usb));
    osKernelInitialize(); // TOS Tiny kernel initialize
    #if(DEMO1==1)
    osThreadCreate(osThread(task1), NULL); // Create task1
    osThreadCreate(osThread(task2), NULL); // Create task2
    #endif
    //osThreadCreate(osThread(task3), NULL); // Create task3
    //osThreadCreate(osThread(task4), NULL); // Create task4
    #if(DEMOTEST==1)
    tasktest=osThreadCreate(osThread(application_entry),NULL); // Kernel test
    #endif
    osKernelStart(); // Start TOS Tiny
    while (1)
        ;
}
